#ifndef RSIM_INTEGRATOR_H
#define RSIM_INTEGRATOR_H

/* -------------------------------------------------------------------------- *
 * File: Integrator.h                                                         *
 * Authors: Kishor Bhalerao                                                   *
 * Email : kishor8dm@gmail.com                                                *
 * Contributors:                                                              *
 * Email:                                                                     *
 * copyright (c) 2010 Authors.                                                *
 *                                                                            *
 * This program is free software: you can redistribute it and/or modify it    *
 * under the terms of the GNU General Public License as published by the Free *
 * Software Foundation, either version 3 of the License, or any later version.*
 *                                                                            *
 * This program is distributed in the hope that it will be useful, but        *
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
 * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License   *
 * for more details. (http://www.gnu.org/licenses/)                           *
 * -------------------------------------------------------------------------- */


#include <iostream>
#include <cstdlib>

using std::cout;

namespace RSIM{

template <class T> class Vector_;
typedef Vector_<double> Vector;

class MultibodySystem;
class State;
class Reporter;
class SystemData;

/** Solves the Ode y' = f(t,y) with the initial condition y(t0) = y0 over the time period
t_initial to t_final. The procedure implemented here is described in 
<P>
@book{hairer1993,
title={Solving ordinary differential equations: Nonstiff problems},
author={Hairer, E. and N{\o}rsett, S.P. and Wanner, G.},
year={1993},
publisher={Springer}
}
<P>
in chapter II.4, Practical Error Estimation and Step-Size Selection. */ 

class Integrator{
        private:
                double estimateInitialTimeStep(const double& t_initial, 
                                                const double& t_final, 
                                                const int& error_order,
                                                const Vector& Y, 
                                                const Vector& YDot);

                /** Maximum time-step, default value = 1.0 */
                double hmax_;
                
                /** Minimum time-step, 1.0E-15*/
                double hmin_;
                
                Reporter *RepObj_;
                
                void extractYFromState( const SystemData& Data,
                                        const Vector_<int>& NY,
                                        Vector& Y)const;
                
                void extractYDotFromState(const SystemData& Data,
                                          const Vector_<int>& NY,
                                          Vector& YDot)const;
                                          
                void calcSystemAcceleration(SystemData& Data);
        public:
                //TODO: Allow user to specify the initial time-step. In this version, the initial time step
                // even if specified in function argument is ignored.
                /** 
                Integrates the function y' = f(t,y) from time t_initial to t_final. All integration
                parameters must be set before calling this function. 
                @param t_inital Starting value of time
                @param t_final Final value of time
                @param Y0 Initial state vector
                @param deltaT Initial Time-step
                @param writeStateVectorEveryNSteps positive integer. State vector written to the specified file
                */
                void stepTo(void(*calcYDot)(	const double& ,	// Time (t)
                                                const Vector&, 	// Vector Y(t)
                                                Vector& 	// Vector YDot(t)
                                        ),
                        const double& t_initial,
                        const double& t_final,
                        const Vector& Y0,
                        const int& writeStateVectorAtEachTimeStep=0,
                        const char FileName[]="rsimout",
                        const double& deltaT = 1.0E-3
                        );
                
                /** 
                Integrates the MultibodySystem from time t_initial to t_final. All integration
                parameters must be set before calling this function. This function writes the 
                state vector of the system along with the appropriate time-stamp by default to a file
                "y". Refine factor can be specifed to control the number of output points similar to Matlab
                function ode45
                @param t_inital Starting value of time
                @param t_final Final value of time		
                @param writeStateVectorEveryNSteps positive integer. State vector written to the specified file
                */
                void stepTo(const double& t_initial,
                        const double& t_final,		
                        MultibodySystem& system, 
                        State& state,
                        const int& writeStateVectorAtEachTimeStep=1,
                        const char FileName[]="y",
                        const double& deltaT = 1.0E-3);
                
                /** 
                Integrates the MultibodySystem from time t_initial to t_final. All integration
                parameters must be set before calling this function. Reporters can be used with
                this function to get custom output. See documentation for class Reporter for more 
                details.
                */
                void stepTo(const double& t_final,		
                        MultibodySystem& system, 
                        State& state);
                        
                void stepTo(const double& t_final,
                            SystemData& Data);

                /** Set Relative tolerance */
                void setRelTol(const double& RelTol){
                        RelTol_ = RelTol;Threshold_ = AbsTol_/RelTol_;
                }

                /** Set Absolute tolerance */
                void setAbsTol(const double& AbsTol){
                        AbsTol_ = AbsTol;Threshold_ = AbsTol_/RelTol_;
                }

                /** Set the refine factor*/
                void setRefineFactor(const int& refine){
                        Refine_ = refine;
                }
                
                /** Set the maximum time-step. Default value = 1.0*/
                void setHMax(const double& hmax){
                        hmax_ = hmax;
                }
                
                /** Set the minimum time-step. Default value = 1.0E-15*/
                void setHMin(const double& hmin){
                        hmin_ = hmin;
                }

                /** 
                Number of times the integrator will scale down the time step in efforts to meet the 
                specified tolerances. 
                */
                void setMaxNumCuts(const int& num){
                        MaxNumCuts_ = num;
                }		
                
                /** Add reporter*/
                void addReporter(Reporter *rep){
                        RepObj_ = rep;
                }
                
                ~Integrator();
                
        protected:
                virtual void attemptSingleStep(void(*calcYDot)(	const double& ,	// Current time (t)
                                                                const Vector&, 	// Vector Y(t)
                                                                Vector& 	// Vector dY/dt
                                                                ),
                                        const double& CurrentTime, // given: t
                                        const Vector& Y0, 	  // given: Y(t)
                                        const Vector& Y0Dot,	  // Ydot computed at time t0.
                                        const double& deltaT, 	  // given: deltaT
                                        Vector& Y1, 	  	  // calculate:Y(t+deltaT)
                                        Vector& Y1Dot, 	          // calculate:YDot(t+deltaT) This is fed back into the code at the next time step
                                        double& error		  // calculate: estimate of local error in this step.
                ){
                        cout<<"Derieved class must provide an implementation of void attemptSingleStep() for this to work.\n";
                        exit(0);
                };
                
                /** 
                @param CurrentTime: time at the start of the time-step denoted by t0
                @param Y0: State Vector at time t0
                @param Y0Dot: dy/dt evaluated at t0
                @param deltaT: time-step
                @param Y1: State Vector at time t0 + deltaT
                @param Y1Dot: dy/dt evaluated at t0+deltaT
                @param error: local error in this time step
                */
                virtual void attemptSingleStep(const double& CurrentTime, 	// t0
                                        const Vector& Y0, 		// Y(t0)
                                        const Vector& Y0Dot,		// dy/dt evaluated at t0
                                        const double& deltaT,		// timeStep
                                        MultibodySystem& MBS,		
                                        State& state,
                                        Vector& Y1,			// Y(t+deltaT)
                                        Vector& Y1Dot,			// dy/dt evaluated at t+deltaT
                                        double& error 			// local error in this time-step
                                        ){
                        cout<<"Derieved class must provide an implementation of void attemptSingleStep() for this to work.\n";
                        exit(0);
                }
                
                /** 
                @param CurrentTime: time at the start of the time-step denoted by t0
                @param Y0: State Vector at time t0
                @param Y0Dot: dy/dt evaluated at t0
                @param deltaT: time-step
                @param Y1: State Vector at time t0 + deltaT
                @param Y1Dot: dy/dt evaluated at t0+deltaT
                @param error: local error in this time step
                */
                virtual void attemptSingleStep(const double& CurrentTime,       // t0
                                               const Vector& Y0,               // Y(t0)
                                               const Vector& Y0Dot,            // dy/dt evaluated at t0
                                               const double& deltaT,           // timeStep
                                               SystemData& Data,
                                               Vector& Y1,                     // Y(t+deltaT)
                                               Vector& Y1Dot,                  // dy/dt evaluated at t+deltaT
                                               double& error                   // local error in this time-step
                                               ){
                        cout<<"Derieved class must provide an implementation of void attemptSingleStep() for this to work.\n";
                        exit(0);
                }

                /** 
                During integration, the values at time t and t+h are already computed. this function computes y(t_interp),
                where t0 < t_interp < t0+h
                @param t0 time at the start of each time step
                @param y0 State vector at time t0
                @param y0Dot dy/dt(t0)
                @param y1Dot dy/dt(t0+h)
                @param t_interp is evenly spaced between t0 and t0+deltaT
                @param y_interp is state vector evaluated at t_interp
                */
                virtual void interpolate(const double& t0, 
                                        const Vector& y0, 
                                        const Vector& y0Dot,
                                        const Vector& y1Dot,
                                        const double& h, 
                                        const double& t_interp, 
                                        Vector& y_interp){
                        if(Refine_>1){
                                cout<<"Implementation for void interpolate() is missing in the derived class\n"<<
                                "Either set Refine_ = 1 using setRefineFactor(1) or remove the reporter object or "<<
                                "provide an implementation for the function"<<
                                " void interpolate() in the derived class\n";
                                exit(0);
                        }
                }

                /** Estimate the local error in this step*/
                virtual void estimateLocalStepError(const Vector& Y0, 
                                                const Vector& Y1,
                                                const Vector& Yerr,
                                                const double& current_time_step, 
                                                double& error);
                
                /** Error order of this integrator. Provided by the derived class */
                virtual const int getErrorOrder()const=0;
                

                /** This adjusts the step size based on the local error estimate.
                @param error error returned by the chosen integrator.
                @param h current time-step
                @param h_new adjusted time-step
                */
                virtual void adjustStepSize(    const double& error,    // Error estimate. This is obtained from the integrator in use.
                                                const int& ErrorOrder, 	// Order of the integrator error
                                                const double& h, 	// Current h
                                                bool& noFailInThisStep,	// false if there is atleast one failure in meeting the required error tolerances otherwise true
                                                double& h_new 		// new time step
                                );

                /** 
                This can be used to pre-allocate space required for integration by the derived class.
                */
                virtual void initIntegrator(const Vector& Y){}

                /** 
                Default integrator parameters set are as below
                AbsTolerance = 1.0E-6, (default value used by Matlab ode45)
                Relative Tolerance = 1.0E-3, (default value used by Matlab ode45)
                MaxNumCuts = 20,
                MaxScaleFactor = 4,
                MinScaleFactor = 0.1
                hmax = 1.0
                error_order is specified by the derived class
                */
                Integrator();				

                // number of elements in the Y vector
                int numY_;

                /** Absolute tolerance. Must be positive. Default value is 1.0E-6*/
                double AbsTol_;

                /** Relative tolerance. Must be positive. Default value is 1.0E-3 */
                double RelTol_;

                /** Threshold = AbsTol/RelTol */
                double Threshold_;

                /** 
                Number of times the integrator will scale down the time step in efforts to meet the 
                specified tolerances.  
                */
                int MaxNumCuts_;

                /** A time step can at most increase by MaxScaleFactor between consecutive steps */
                int MaxScaleFactor_;

                /** A time step can at most decrease by MinScaleFactor between consecutive steps */
                double MinScaleFactor_;
                
                /** 
                Set refine factor to get values in between the integration steps. If Refine is greater than 1, then the derived class
                must provide in implementation of the virtual function interpolate. The base class does not have any default interpolation 
                functions yet. This is to be added in future. 
                */
                int Refine_;
                                
}; // class Integrator

}// namespace RSIM

#endif
